#include <math.h>

#include "fly.h"
#include "scope_protocol.h"
#include "mpu6xxx.h"

#define T (0.02f) //采样周期

static rt_thread_t imu_thread = RT_NULL; /* 姿态更新线程 */
static rt_sem_t imu_sem = RT_NULL;       /* imu更新信号量 */

struct vector_3d attitude = {0};

rt_int32_t pwm_left = 0, pwm_right = 0;

static void imu_period_callback()
{
    rt_sem_release(imu_sem);
}

float invSqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long *)&y;
    i = 0x5f3759df - (i >> 1);
    y = *(float *)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

/**
 * 六轴姿态融合
 */
void MahonyAHRSupdateIMU(struct mpu6xxx_3axes *accel, struct mpu6xxx_3axes *gyro, struct vector_3d *result)
{
    static float Kp = 8.0f, Ki = 0.05f;
    static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
    static float integral_x = 0.0f, integral_y = 0.0f, integral_z = 0.0f;

    float ax, ay, az, gx, gy, gz;
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;
    float qa, qb, qc, qd; //四元数缓存

    //角速度单位转换
    gx = gyro->x / 10.0f / 57.3f;
    gy = gyro->y / 10.0f / 57.3f;
    gz = gyro->z / 10.0f / 57.3f;

    if (!((accel->x == 0.0f) && (accel->y == 0.0f) && (accel->z == 0.0f)))
    {
        // 加速度计规范化
        recipNorm = invSqrt(accel->x * accel->x + accel->y * accel->y + accel->z * accel->z);
        ax = accel->x * recipNorm;
        ay = accel->y * recipNorm;
        az = accel->z * recipNorm;
        // 重力加速度坐标系变换
        halfvx = q1 * q3 - q0 * q2;
        halfvy = q0 * q1 + q2 * q3;
        halfvz = q0 * q0 - 0.5f + q3 * q3;
        // 计算误差
        halfex = (ay * halfvz - az * halfvy);
        halfey = (az * halfvx - ax * halfvz);
        halfez = (ax * halfvy - ay * halfvx);

        // Compute and apply integral feedback if enabled
        if (Ki > 0.0f)
        {
            integral_x += Ki * halfex * T; // integral error scaled by Ki
            integral_y += Ki * halfey * T;
            integral_z += Ki * halfez * T;
        }
        else
        {
            integral_x = 0.0f;
            integral_y = 0.0f;
            integral_z = 0.0f;
        }

        // Apply proportional feedback
        gx += Kp * halfex + integral_x;
        gy += Kp * halfey + integral_y;
        gz += Kp * halfez + integral_z;
    }

    // 一阶龙格库塔法
    qa = q0;
    qb = q1;
    qc = q2;
    qd = q3;
    q0 += (-qb * gx - qc * gy - qd * gz) * (0.5f * T);
    q1 += (qa * gx + qc * gz - qd * gy) * (0.5f * T);
    q2 += (qa * gy - qb * gz + qd * gx) * (0.5f * T);
    q3 += (qa * gz + qb * gy - qc * gx) * (0.5f * T);

    // 四元数规范化
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;

    q2 *= recipNorm;
    q3 *= recipNorm;

    result->x = asinf(-2.0f * q1 * q3 + 2 * q0 * q2) * 57.3;                                  // pitch
    result->y = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;      // roll
    result->z = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; // yaw
    protocol_report_data(PROTOCOL_CHANNEL_1, result->x);
    protocol_report_data(PROTOCOL_CHANNEL_2, result->y);
    // protocol_report_data(PROTOCOL_CHANNEL_3, result->z);
}

/**
 * 电机PID控制
 */
static void pid_control()
{
    static float roll_error, pitch_error, yaw_error;       //误差
    static float roll_error_i, pitch_error_i, yaw_error_i; //误差积分

    static float roll_w_desire, pitch_w_desire, yaw_w_desire;    //外环PI期望的角速度
    static float roll_w_error, pitch_w_error, yaw_w_error;       //角速度误差
    static float roll_w_error_i, pitch_w_error_i, yaw_w_error_i; //角速度误差积分

    static float last_roll_w_error, last_pitch_w_error, last_yaw_w_error; //上次角速度误差

    roll_error = (control.roll_target - control.roll_current);
    pitch_error = (control.pitch_target - control.pitch_current);

    roll_error_i += roll_error;
    pitch_error_i += pitch_error;

    //积分限幅
    if (roll_error_i > 500)
        roll_error_i = 500;
    if (roll_error_i < -500)
        roll_error_i = -500;
    if (pitch_error_i > 500)
        pitch_error_i = 500;
    if (pitch_error_i < -500)
        pitch_error_i = -500;

    //外环PI产生期望的角速度
    roll_w_desire = (roll_error)*info.roll_p + roll_error_i * info.roll_i;
    pitch_w_desire = (pitch_error)*info.pitch_p + pitch_error_i * info.pitch_i;

    //外环PI得到的期望角速度减去当前角速度，得到角速度误差
    roll_w_error = roll_w_desire - control.roll_w_current;
    pitch_w_error = pitch_w_desire - control.pitch_w_current;
    //误差积分
    roll_w_error_i += roll_w_error;
    pitch_w_error_i += pitch_w_error;

    //积分限幅
    if (roll_w_error_i > 500)
        roll_w_error_i = 500;
    if (roll_w_error_i < -500)
        roll_w_error_i = -500;
    if (pitch_w_error_i > 500)
        pitch_w_error_i = 500;
    if (pitch_w_error_i < -500)
        pitch_w_error_i = -500;

    control.roll_out = -roll_w_error * info.roll_p_in + roll_w_error_i * info.roll_i_in - (roll_w_error - last_roll_w_error) * info.roll_d_in;
    control.pitch_out = -pitch_w_error * info.pitch_p_in + pitch_w_error_i * info.pitch_i_in - (pitch_w_error - last_pitch_w_error) * info.pitch_d_in;

    last_roll_w_error = roll_w_error;
    last_pitch_w_error = pitch_w_error;
}

static void imu_entry()
{
    static rt_err_t ret;

    struct mpu6xxx_device *dev;
    struct mpu6xxx_3axes accel, gyro;
    static rt_timer_t timer1;
    static int32_t encoderLeft, encoderRight;
    rt_int32_t pwm_balance, pwm_velocity, pwm_turn;

    /* Initialize mpu6xxx, The parameter is RT_NULL, means auto probing for i2c*/
    dev = mpu6xxx_init("i2c1", RT_NULL);
    if (dev == RT_NULL)
    {
        rt_kprintf("mpu6xxx init failed\n");
        return;
    }
    imu_sem = rt_sem_create("imu_sem", 1, RT_IPC_FLAG_PRIO);

    /* 创建定时器 1  周期定时器 */
    timer1 = rt_timer_create("timer1", imu_period_callback,
                             RT_NULL, T * 1000,
                             RT_TIMER_FLAG_PERIODIC);
    rt_timer_start(timer1);

    while (1)
    {
        ret = rt_sem_take(imu_sem, RT_WAITING_FOREVER);

        mpu6xxx_get_accel(dev, &accel);
        mpu6xxx_get_gyro(dev, &gyro);

        MahonyAHRSupdateIMU(&accel, &gyro, &attitude);

        control.roll_w_current = gyro.x / 10.0f / 57.3f;
        control.pitch_w_current = gyro.y / 10.0f / 57.3f;
        control.yaw_w_current = gyro.z / 10.0f / 57.3f;

        control.roll_current = attitude.x;
        control.pitch_current = attitude.y;
        control.yaw_current = attitude.z;
        if (abs(attitude.x) > 30 || abs(attitude.y) > 30)
        {
            motor_lock();
        }

        pid_control();

        control.motor_pwm_out1 = MOTOR_PWM_MIN + control.throttle + 5000.0f*(control.pitch_out + control.roll_out + control.yaw_out);
        control.motor_pwm_out2 = MOTOR_PWM_MIN + control.throttle + 5000.0f*(control.pitch_out - control.roll_out - control.yaw_out);
        control.motor_pwm_out3 = MOTOR_PWM_MIN + control.throttle + 5000.0f*(-control.pitch_out - control.roll_out + control.yaw_out);
        control.motor_pwm_out4 = MOTOR_PWM_MIN + control.throttle + 5000.0f*(-control.pitch_out + control.roll_out - control.yaw_out);
        //电机输出限幅
        if (control.motor_pwm_out1 > MOTOR_PWM_MAX)
            control.motor_pwm_out1 = MOTOR_PWM_MAX;
        if (control.motor_pwm_out1 < MOTOR_PWM_MIN)
            control.motor_pwm_out1 = MOTOR_PWM_MIN;
        if (control.motor_pwm_out2 > MOTOR_PWM_MAX)
            control.motor_pwm_out2 = MOTOR_PWM_MAX;
        if (control.motor_pwm_out2 < MOTOR_PWM_MIN)
            control.motor_pwm_out2 = MOTOR_PWM_MIN;
        if (control.motor_pwm_out3 > MOTOR_PWM_MAX)
            control.motor_pwm_out3 = MOTOR_PWM_MAX;
        if (control.motor_pwm_out3 < MOTOR_PWM_MIN)
            control.motor_pwm_out3 = MOTOR_PWM_MIN;
        if (control.motor_pwm_out4 > MOTOR_PWM_MAX)
            control.motor_pwm_out4 = MOTOR_PWM_MAX;
        if (control.motor_pwm_out4 < MOTOR_PWM_MIN)
            control.motor_pwm_out4 = MOTOR_PWM_MIN;

        // protocol_report_data(PROTOCOL_CHANNEL_1, control.motor_pwm_out1);
        // protocol_report_data(PROTOCOL_CHANNEL_3, control.motor_pwm_out2);

        /**
         * TODO:此处需要做电源补偿，同样占空比在不同电压下输出不同，需要做补偿
         */
        // if (info.power > 11.1f) //欠压保护
        if (info.is_unlock)
        {
            set_motor_pwm(control.motor_pwm_out1, control.motor_pwm_out2, control.motor_pwm_out3, control.motor_pwm_out4);
        }
    }

    mpu6xxx_deinit(dev);
}

static int mpu6050_init(const char *name)
{
    imu_thread = rt_thread_create("imu_thread",
                                  imu_entry,
                                  RT_NULL,
                                  THREAD_STACK_SIZE,
                                  THREAD_PRIORITY,
                                  THREAD_TIMESLICE);
    if (imu_thread != RT_NULL)
    {
        rt_thread_startup(imu_thread);
    }
    return 0;
}
INIT_APP_EXPORT(mpu6050_init);
